#include <ros/ros.h> 
#include <serial/serial.h>  //ROS已经内置了的串口�?
#include <std_msgs/String.h>
#include <std_msgs/Empty.h> 
#include <string>
#include <vector>
#include <sstream>
#include <cmath>
#include <cstdlib>  //string转化为double
#include <iomanip>  //保留有效小数
#include<iostream>  
#include <custom_msg/CMD.h>
#include "commons/rabbit_consts.h"

using namespace std;
serial::Serial ser; //声明串口对象

custom_msg::CMD CMD_data;//全局变量，解析后数据
int cmd_change_old = MSG_CMD_TYPE_SUMBOY_RGB;
int cmd_focus_old = MSG_CMD_TYPE_SUMBOY_RGB_FOCUS1;

void chatterCallback(const custom_msg::CMDConstPtr& msg)
{
    int cmd_type = msg->cmd_type;
    int val_type = msg->val_type;
    if(cmd_type==MSG_CMD_TYPE_SUMBOY){
      switch(val_type){
        case MSG_CMD_TYPE_SUMBOY_HW:
        if(cmd_change_old != MSG_CMD_TYPE_SUMBOY_HW){
          cmd_change_old = MSG_CMD_TYPE_SUMBOY_HW;
          ser.write(SUMBOY_CMD_HW,SUMBOY_CMD_LEN);
        }
        break;
        case MSG_CMD_TYPE_SUMBOY_RGB:
        if(cmd_change_old != MSG_CMD_TYPE_SUMBOY_RGB){
          cmd_change_old = MSG_CMD_TYPE_SUMBOY_RGB;
          ser.write(SUMBOY_CMD_RGB,SUMBOY_CMD_LEN);
        }
        break;                                      //change hw or rgb
        case MSG_CMD_TYPE_SUMBOY_HW_FOCUS1:
        if(cmd_focus_old != MSG_CMD_TYPE_SUMBOY_HW_FOCUS1){
          cmd_focus_old = MSG_CMD_TYPE_SUMBOY_HW_FOCUS1;
          ser.write(SUMBOY_CMD_HW_FOCUS1,SUMBOY_CMD_LEN);
        }
        break;      
        case MSG_CMD_TYPE_SUMBOY_HW_FOCUS2:
        if(cmd_focus_old != MSG_CMD_TYPE_SUMBOY_HW_FOCUS2){
          cmd_focus_old = MSG_CMD_TYPE_SUMBOY_HW_FOCUS2;
          ser.write(SUMBOY_CMD_HW_FOCUS2,SUMBOY_CMD_LEN);
        }
        break;  
        case MSG_CMD_TYPE_SUMBOY_HW_FOCUS3:
        if(cmd_focus_old != MSG_CMD_TYPE_SUMBOY_HW_FOCUS3){
          cmd_focus_old = MSG_CMD_TYPE_SUMBOY_HW_FOCUS3;
          ser.write(SUMBOY_CMD_HW_FOCUS3,SUMBOY_CMD_LEN);
        }
        break;  
        case MSG_CMD_TYPE_SUMBOY_HW_FOCUS4:
        if(cmd_focus_old != MSG_CMD_TYPE_SUMBOY_HW_FOCUS4){
          cmd_focus_old = MSG_CMD_TYPE_SUMBOY_HW_FOCUS4;
          ser.write(SUMBOY_CMD_HW_FOCUS4,SUMBOY_CMD_LEN);
        }
        break;  
        case MSG_CMD_TYPE_SUMBOY_HW_FOCUS5:
        if(cmd_focus_old != MSG_CMD_TYPE_SUMBOY_HW_FOCUS5){
          cmd_focus_old = MSG_CMD_TYPE_SUMBOY_HW_FOCUS5;
          ser.write(SUMBOY_CMD_HW_FOCUS5,SUMBOY_CMD_LEN);
        }
        break;    
        case MSG_CMD_TYPE_SUMBOY_HW_FOCUS6:
        if(cmd_focus_old != MSG_CMD_TYPE_SUMBOY_HW_FOCUS6){
          cmd_focus_old = MSG_CMD_TYPE_SUMBOY_HW_FOCUS6;
          ser.write(SUMBOY_CMD_HW_FOCUS6,SUMBOY_CMD_LEN);
        }
        break;                                   //hw focus 
        case MSG_CMD_TYPE_SUMBOY_RGB_FOCUS1:
        if(cmd_focus_old != MSG_CMD_TYPE_SUMBOY_RGB_FOCUS1){
          cmd_focus_old = MSG_CMD_TYPE_SUMBOY_RGB_FOCUS1;
          ser.write(SUMBOY_CMD_RGB_FOCUS1,SUMBOY_CMD_RGB_LEN);
        }
        break;    
        case MSG_CMD_TYPE_SUMBOY_RGB_FOCUS2:
        if(cmd_focus_old != MSG_CMD_TYPE_SUMBOY_RGB_FOCUS2){
          cmd_focus_old = MSG_CMD_TYPE_SUMBOY_RGB_FOCUS2;
          ser.write(SUMBOY_CMD_RGB_FOCUS2,SUMBOY_CMD_RGB_LEN);
        }
        break; 
        case MSG_CMD_TYPE_SUMBOY_RGB_FOCUS3:
        if(cmd_focus_old != MSG_CMD_TYPE_SUMBOY_RGB_FOCUS3){
          cmd_focus_old = MSG_CMD_TYPE_SUMBOY_RGB_FOCUS3;
          ser.write(SUMBOY_CMD_RGB_FOCUS3,SUMBOY_CMD_RGB_LEN);
        }
        break; 
        case MSG_CMD_TYPE_SUMBOY_RGB_FOCUS4:
        if(cmd_focus_old != MSG_CMD_TYPE_SUMBOY_RGB_FOCUS4){
          cmd_focus_old = MSG_CMD_TYPE_SUMBOY_RGB_FOCUS4;
          ser.write(SUMBOY_CMD_RGB_FOCUS4,SUMBOY_CMD_RGB_LEN);
        }
        break;  
        case MSG_CMD_TYPE_SUMBOY_RGB_FOCUS5:
        if(cmd_focus_old != MSG_CMD_TYPE_SUMBOY_RGB_FOCUS5){
          cmd_focus_old = MSG_CMD_TYPE_SUMBOY_RGB_FOCUS5;
          ser.write(SUMBOY_CMD_RGB_FOCUS5,SUMBOY_CMD_RGB_LEN);
        }
        break; 
        case MSG_CMD_TYPE_SUMBOY_RGB_FOCUS6:
        if(cmd_focus_old != MSG_CMD_TYPE_SUMBOY_RGB_FOCUS6){
          cmd_focus_old = MSG_CMD_TYPE_SUMBOY_RGB_FOCUS6;
          ser.write(SUMBOY_CMD_RGB_FOCUS6,SUMBOY_CMD_RGB_LEN);
        }
        break;                                //rgb focus                  
      }
    }
}

int main(int argc, char** argv)
{
  uint8_t recieve_data[48] = {0};
  int len = 0;

  ros::init(argc, argv, "serial_node_sumboy");
  ros::NodeHandle nh;
  ros::Subscriber sub = nh.subscribe(TOPIC_CMD, 1000, chatterCallback);
  try
  {
    ser.setPort(SERIAL_SUMBOY_DEV);
    ser.setBaudrate(115200);
    serial::Timeout to = serial::Timeout::simpleTimeout(1000);
    ser.setTimeout(to);
    ser.open();
  }
  catch (serial::IOException& e)
  {
    ROS_ERROR_STREAM("Unable to open Serial Port !");
    return -1;
  }
  if (ser.isOpen())
  {
    ROS_INFO_STREAM("Serial Port initialized");
  }
  else
  {
    return -1;
  }
  ser.write(SUMBOY_CMD_RGB,SUMBOY_CMD_LEN); //给个默认值
  ros::Rate loop_rate(10);
  while (ros::ok())
  {  
   // len = ser.write(CMD_INF,48);
  //  cout << "write CMD_INF len:" << len<< "\r\n"; 
    //len_receive = ser.read(recieve_data, 48);
    ros::spinOnce();
    loop_rate.sleep();
  }
}


